#include "vecmath.h"

void MultiplyRightMat4f(Mat4f out, const Mat4f right) {
    float lrxc0 = out[0][0]; float lrxc1 = out[0][1]; float lrxc2 = out[0][2]; float lrxc3 = out[0][3];
    out[0][0] = lrxc0 * right[0][0] + lrxc1 * right[1][0] + lrxc2 * right[2][0] + lrxc3 * right[3][0];
    out[0][1] = lrxc0 * right[0][1] + lrxc1 * right[1][1] + lrxc2 * right[2][1] + lrxc3 * right[3][1];
    out[0][2] = lrxc0 * right[0][2] + lrxc1 * right[1][2] + lrxc2 * right[2][2] + lrxc3 * right[3][2];
    out[0][3] = lrxc0 * right[0][3] + lrxc1 * right[1][3] + lrxc2 * right[2][3] + lrxc3 * right[3][3];
    
    lrxc0 = out[1][0]; lrxc1 = out[1][1]; lrxc2 = out[1][2]; lrxc3 = out[1][3];
    out[1][0] = lrxc0 * right[0][0] + lrxc1 * right[1][0] + lrxc2 * right[2][0] + lrxc3 * right[3][0];
    out[1][1] = lrxc0 * right[0][1] + lrxc1 * right[1][1] + lrxc2 * right[2][1] + lrxc3 * right[3][1];
    out[1][2] = lrxc0 * right[0][2] + lrxc1 * right[1][2] + lrxc2 * right[2][2] + lrxc3 * right[3][2];
    out[1][3] = lrxc0 * right[0][3] + lrxc1 * right[1][3] + lrxc2 * right[2][3] + lrxc3 * right[3][3];
    
    lrxc0 = out[2][0]; lrxc1 = out[2][1]; lrxc2 = out[2][2]; lrxc3 = out[2][3];
    out[2][0] = lrxc0 * right[0][0] + lrxc1 * right[1][0] + lrxc2 * right[2][0] + lrxc3 * right[3][0];
    out[2][1] = lrxc0 * right[0][1] + lrxc1 * right[1][1] + lrxc2 * right[2][1] + lrxc3 * right[3][1];
    out[2][2] = lrxc0 * right[0][2] + lrxc1 * right[1][2] + lrxc2 * right[2][2] + lrxc3 * right[3][2];
    out[2][3] = lrxc0 * right[0][3] + lrxc1 * right[1][3] + lrxc2 * right[2][3] + lrxc3 * right[3][3];
    
    lrxc0 = out[3][0]; lrxc1 = out[3][1]; lrxc2 = out[3][2]; lrxc3 = out[3][3];
    out[3][0] = lrxc0 * right[0][0] + lrxc1 * right[1][0] + lrxc2 * right[2][0] + lrxc3 * right[3][0];
    out[3][1] = lrxc0 * right[0][1] + lrxc1 * right[1][1] + lrxc2 * right[2][1] + lrxc3 * right[3][1];
    out[3][2] = lrxc0 * right[0][2] + lrxc1 * right[1][2] + lrxc2 * right[2][2] + lrxc3 * right[3][2];
    out[3][3] = lrxc0 * right[0][3] + lrxc1 * right[1][3] + lrxc2 * right[2][3] + lrxc3 * right[3][3];
}

void MultiplyLeftMat4f(const Mat4f left, Mat4f out) {
    float rr0cx = out[0][0]; float rr1cx = out[1][0]; float rr2cx = out[2][0]; float rr3cx = out[3][0];
    out[0][0] = left[0][0] * rr0cx + left[0][1] * rr1cx + left[0][2] * rr2cx + left[0][3] * rr3cx;
    out[1][0] = left[1][0] * rr0cx + left[1][1] * rr1cx + left[1][2] * rr2cx + left[1][3] * rr3cx;
    out[2][0] = left[2][0] * rr0cx + left[2][1] * rr1cx + left[2][2] * rr2cx + left[2][3] * rr3cx;
    out[3][0] = left[3][0] * rr0cx + left[3][1] * rr1cx + left[3][2] * rr2cx + left[3][3] * rr3cx;
    
    rr0cx = out[0][1]; rr1cx = out[1][1]; rr2cx = out[2][1]; rr3cx = out[3][1];
    out[0][1] = left[0][0] * rr0cx + left[0][1] * rr1cx + left[0][2] * rr2cx + left[0][3] * rr3cx;
    out[1][1] = left[1][0] * rr0cx + left[1][1] * rr1cx + left[1][2] * rr2cx + left[1][3] * rr3cx;
    out[2][1] = left[2][0] * rr0cx + left[2][1] * rr1cx + left[2][2] * rr2cx + left[2][3] * rr3cx;
    out[3][1] = left[3][0] * rr0cx + left[3][1] * rr1cx + left[3][2] * rr2cx + left[3][3] * rr3cx;
    
    rr0cx = out[0][2]; rr1cx = out[1][2]; rr2cx = out[2][2]; rr3cx = out[3][2];
    out[0][2] = left[0][0] * rr0cx + left[0][1] * rr1cx + left[0][2] * rr2cx + left[0][3] * rr3cx;
    out[1][2] = left[1][0] * rr0cx + left[1][1] * rr1cx + left[1][2] * rr2cx + left[1][3] * rr3cx;
    out[2][2] = left[2][0] * rr0cx + left[2][1] * rr1cx + left[2][2] * rr2cx + left[2][3] * rr3cx;
    out[3][2] = left[3][0] * rr0cx + left[3][1] * rr1cx + left[3][2] * rr2cx + left[3][3] * rr3cx;
    
    rr0cx = out[0][3]; rr1cx = out[1][3]; rr2cx = out[2][3]; rr3cx = out[3][3];
    out[0][3] = left[0][0] * rr0cx + left[0][1] * rr1cx + left[0][2] * rr2cx + left[0][3] * rr3cx;
    out[1][3] = left[1][0] * rr0cx + left[1][1] * rr1cx + left[1][2] * rr2cx + left[1][3] * rr3cx;
    out[2][3] = left[2][0] * rr0cx + left[2][1] * rr1cx + left[2][2] * rr2cx + left[2][3] * rr3cx;
    out[3][3] = left[3][0] * rr0cx + left[3][1] * rr1cx + left[3][2] * rr2cx + left[3][3] * rr3cx;
}

void MultiplyMat4f(Mat4f out, const Mat4f a, const Mat4f b) {
    float lrxc0 = a[0][0]; float lrxc1 = a[0][1]; float lrxc2 = a[0][2]; float lrxc3 = a[0][3];
    out[0][0] = lrxc0 * b[0][0] + lrxc1 * b[1][0] + lrxc2 * b[2][0] + lrxc3 * b[3][0];
    out[0][1] = lrxc0 * b[0][1] + lrxc1 * b[1][1] + lrxc2 * b[2][1] + lrxc3 * b[3][1];
    out[0][2] = lrxc0 * b[0][2] + lrxc1 * b[1][2] + lrxc2 * b[2][2] + lrxc3 * b[3][2];
    out[0][3] = lrxc0 * b[0][3] + lrxc1 * b[1][3] + lrxc2 * b[2][3] + lrxc3 * b[3][3];
    
    lrxc0 = a[1][0]; lrxc1 = a[1][1]; lrxc2 = a[1][2]; lrxc3 = a[1][3];
    out[1][0] = lrxc0 * b[0][0] + lrxc1 * b[1][0] + lrxc2 * b[2][0] + lrxc3 * b[3][0];
    out[1][1] = lrxc0 * b[0][1] + lrxc1 * b[1][1] + lrxc2 * b[2][1] + lrxc3 * b[3][1];
    out[1][2] = lrxc0 * b[0][2] + lrxc1 * b[1][2] + lrxc2 * b[2][2] + lrxc3 * b[3][2];
    out[1][3] = lrxc0 * b[0][3] + lrxc1 * b[1][3] + lrxc2 * b[2][3] + lrxc3 * b[3][3];
    
    lrxc0 = a[2][0]; lrxc1 = a[2][1]; lrxc2 = a[2][2]; lrxc3 = a[2][3];
    out[2][0] = lrxc0 * b[0][0] + lrxc1 * b[1][0] + lrxc2 * b[2][0] + lrxc3 * b[3][0];
    out[2][1] = lrxc0 * b[0][1] + lrxc1 * b[1][1] + lrxc2 * b[2][1] + lrxc3 * b[3][1];
    out[2][2] = lrxc0 * b[0][2] + lrxc1 * b[1][2] + lrxc2 * b[2][2] + lrxc3 * b[3][2];
    out[2][3] = lrxc0 * b[0][3] + lrxc1 * b[1][3] + lrxc2 * b[2][3] + lrxc3 * b[3][3];
    
    lrxc0 = a[3][0]; lrxc1 = a[3][1]; lrxc2 = a[3][2]; lrxc3 = a[3][3];
    out[3][0] = lrxc0 * b[0][0] + lrxc1 * b[1][0] + lrxc2 * b[2][0] + lrxc3 * b[3][0];
    out[3][1] = lrxc0 * b[0][1] + lrxc1 * b[1][1] + lrxc2 * b[2][1] + lrxc3 * b[3][1];
    out[3][2] = lrxc0 * b[0][2] + lrxc1 * b[1][2] + lrxc2 * b[2][2] + lrxc3 * b[3][2];
    out[3][3] = lrxc0 * b[0][3] + lrxc1 * b[1][3] + lrxc2 * b[2][3] + lrxc3 * b[3][3];
}

void MultiplyMat4fVec4f(Vec4f out, const Mat4f m, const Vec4f v) {
    out[0] = m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3] * v[3];
    out[1] = m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3] * v[3];
    out[2] = m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3] * v[3];
    out[3] = m[3][0] * v[0] + m[3][1] * v[1] + m[3][2] * v[2] + m[3][3] * v[3];
}

void MultiplyMat4fVec3f(Vec3f out, const Mat4f m, const Vec3f v) {
    float invW = 1.0f / (m[3][0] * v[0] + m[3][1] * v[1] + m[3][2] * v[2] + m[3][3]);
    out[0] = (m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2] + m[0][3]) * invW;
    out[1] = (m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2] + m[1][3]) * invW;
    out[2] = (m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] + m[2][3]) * invW;
}
